#include "prospeckz_radio.h"
#include "StopAndWait.h"
#include "MAC.h"
#include "prospeckz.h"

//#define SCREENDEBUG

#ifdef SCREENDEBUG
#include "lcd.h"
#else
#define lcd_print_char(x) ;
#define lcd_print_cstring(x) ;
#define lcd_set_pos(x,y) ;
#endif


#include "hashmap.h"
#include "dynmem.h"

#define MAX_RX_SIZE 79

#define TRANSMIT_INVALID 0
#define TRANSMIT_IDLE 1
#define TRANSMIT_WAITING 2
#define TRANSMIT_TIMEOUT 3
#define TRANSMIT_ACKD 4
#define TRANSMIT_FAIL_RESET 5

#define RST (2)
#define TR_0 0
#define TR_1 1
#define ACK_0 0
#define ACK_1 1

#define DSP_DATA 0
#define DSP_ACK 1
#define DSP_CHECK 2

size_t hash_addr64(void *key)
{
	struct xlowpan_addr64 *addr = (struct xlowpan_addr64*)key;
	size_t hash = 0;

	if(!key)
		return 0;

	/* TODO: make better hashing function -> use Fletcher-16 ? */

	/* lame */
	{
		size_t i = XLOWPAN_ADDR_LEN;
		while(i--) {
			hash += addr->addr[i] * i;
		}
	}

	return hash;
}

struct reliable_status
{
	unsigned char TransmitStatus;
	char NextSendSeq;
	char NextRecvSeq;
} ;

struct hash_map* reliable_status_map;

unsigned int (*Reliable_SendTarget)(struct xlowpan_addr64* from, void* data, unsigned int length);

void Reliable_SetSendHandler(unsigned int (*X)(struct xlowpan_addr64* to, void* data, unsigned int length))
{
	Reliable_SendTarget = X;
}

struct xlowpan_addr64* Addr;
void Reliable_GetAddr(struct xlowpan_addr64* target)
{
	target->addr[0] = Addr->addr[0];
	target->addr[1] = Addr->addr[1];
	target->addr[2] = Addr->addr[2];
	target->addr[3] = Addr->addr[3];
	target->addr[4] = Addr->addr[4];
	target->addr[5] = Addr->addr[5];
	target->addr[6] = Addr->addr[6];
	target->addr[7] = Addr->addr[7];
}

unsigned char Reliable_MakeSession(void)
{
	static unsigned char session = 0;
	
	return ++session;
}

struct mac_driver layer;

void NullFunction(void){}

void Reliable_Init(struct xlowpan_addr64* addr, unsigned char power)
{
	unsigned char seed = 0;	
	
	reliable_status_map = hmap_create(10, hash_addr64);
	
	Addr = addr;
	
	layer.send_pkt = &MAC_Transmit;
	layer.set_receive_pkt = &MAC_SetHandler;
	layer.get_addr64 = &Reliable_GetAddr;
	layer.make_session = &Reliable_MakeSession;
	layer.init = &NullFunction;
			
	Radio_init(254);
	Radio_setTxStrength(power);
	Radio_receiverOn;
	
	xlowpan_init(&layer);
}

int page = 2;
unsigned char fullPacket[79];
int Reliable_TransmitPacket(struct xlowpan_addr64* to, unsigned char* data, unsigned int length, unsigned long timeout, unsigned int retries)
{
	
	int i = 0;
	int c = 0;
	int p = page++;
	int check_fail = -1;
	unsigned long SendTimeout;

	struct reliable_status* status;

	lcd_set_pos(p,10*c++);
	lcd_print_char('S');
		
	if(length > 76)
	{
		return FAIL_OTHER;
	}
	status = hmap_get(reliable_status_map, to);

	if(status == 0)
	{
		status = (struct reliable_status*)malloc(sizeof(struct reliable_status));
		if(status == 0)
		{
			return FAIL_OTHER;
		}
		status->TransmitStatus = TRANSMIT_IDLE;
		status->NextSendSeq = RST; 
		status->NextRecvSeq = RST;
		hmap_set(reliable_status_map, to, status);
	} 

	fullPacket[0] = DSP_RELIABLE;
	fullPacket[1] = DSP_DATA;
	fullPacket[2] = status->NextSendSeq;

	memcpy(fullPacket+3, data, length);
	status->TransmitStatus = TRANSMIT_WAITING;
		
	Reliable_SendTarget(to, fullPacket,length+3);

	retries--;
	SendTimeout = 0;
		
	while(status->TransmitStatus != TRANSMIT_ACKD)
	{				
		MAC_CheckRadio();
		if(status->TransmitStatus == TRANSMIT_FAIL_RESET) 
		{
			status->TransmitStatus = TRANSMIT_IDLE;
			lcd_set_pos(p,10*c++);
			lcd_print_char('R');
			return FAIL_RESET;
		}
		
		if(SendTimeout++ >= timeout)
		{
			if(retries-- > 0)
			{
				Reliable_SendTarget(to, fullPacket,length+3);
				lcd_set_pos(p,10*c++);
				lcd_print_char('r');
			}
			else
			{
				status->TransmitStatus = TRANSMIT_IDLE;
				lcd_set_pos(p,10*c++);
				lcd_print_char('T');
				return FAIL_TIMEOUT;
			}
			
			SendTimeout = 0;
			timeout += hash_addr64(xlowpan_getaddr()) % 1000;
		}
				
	}
	lcd_set_pos(p,10*c++);
	lcd_print_cstring(":-)");
	status->TransmitStatus = TRANSMIT_IDLE;
	return 0;
}


void Reliable_ReceivePacket(struct xlowpan_addr64* from, unsigned char* data, unsigned char hops, unsigned int length)
{
	int i = 0;
	unsigned char ack[3];
	struct reliable_status *status;

	lcd_set_pos(page++,0);
	
	if(length < 2) 
	{
		return; //no data
	}
	status = hmap_get(reliable_status_map, from);
	
	lcd_print_char('R');
	
	//if we've never encountered this node before, construct a connection for it.
	if(status == 0)
	{
		lcd_print_char('2');
		status = (struct reliable_status*) malloc(sizeof(struct reliable_status));
		if(status == 0)
		{
			lcd_print_char('X');
			return;
		}
		
		status->TransmitStatus = TRANSMIT_IDLE;
		status->NextSendSeq = RST; 
		status->NextRecvSeq = RST; 
		hmap_set(reliable_status_map, from, status);
		lcd_print_char('3');
	}

	if(data[0] == DSP_ACK) //if we've received an ACK
	{
		lcd_print_char('A');
		//if we're waiting for an ACK
		if((status->TransmitStatus == TRANSMIT_WAITING || status->TransmitStatus == TRANSMIT_TIMEOUT))
		{
			lcd_print_char('a');
			if(data[1] == RST) //if this is a reset ACK
			{
				lcd_print_char('r');
				//reset the connection and indicate to the transmittor that the connection has failed
				status->NextSendSeq = RST;
				status->NextRecvSeq = RST;
				status->TransmitStatus = TRANSMIT_FAIL_RESET;
				return;
			}
		
			//if we've been reset and this is a '0' ACK, or this is the correct ACK
			if((status->NextSendSeq == RST && data[1] == ACK_0) || data[1] == status->NextSendSeq)
			{
				lcd_print_char('o');
				//if we're not reset
				if(status->NextSendSeq != RST)
					status->NextSendSeq = ! status->NextSendSeq; //toggle the next sent seq number
				else
					status->NextSendSeq = 1; //otherwise, set it to 1
				status->TransmitStatus = TRANSMIT_ACKD;
				lcd_print_char('k');
				return;
			}
		}
	}
	//if we've received a data packet
	else if(data[0] == DSP_DATA)
	{
		lcd_print_char('D');
		ack[0] = DSP_RELIABLE;
		ack[1] = DSP_ACK;
				
		//if we've been reset, the next received data must carry the reset flag (i.e., reject this data if it does not)
		if(status->NextRecvSeq == RST)
		{
			if(data[1] != RST)
			{
				lcd_print_char('R');
				ack[2] = RST;
				Reliable_SendTarget(from,ack,2);
				xlowpan_resetsession();
				return;
			}
		} 
		
		//if this is the data we're expecting...
		if(data[1] == status->NextRecvSeq || data[1] == RST)
		{
			lcd_print_char('a');
			if(data[1] == RST)
			{
				ack[2] = ACK_0; //if we've been sent a reset packet, the ack should be a 0
				status->NextRecvSeq = ACK_1;
				xlowpan_resetsession();
				lcd_print_char('t');
			}
			else 
			{	
				ack[2] = status->NextRecvSeq; //otherwise, ack the packet we've been sent
				status->NextRecvSeq = !status->NextRecvSeq;
				lcd_print_char('a');
			}
			
			//send the ack down the stack
			Reliable_SendTarget(from, ack, 3);
			//send the data up the stack.
			MAC_Decode(from, data+2, hops,length-2);
		}
		else
		{
			//this is an out of order packet so ack it and discard it.
			ack[2] = ! status->NextRecvSeq;
			lcd_print_char('Z');
			Reliable_SendTarget(from, ack,3);
		}
	}
	//test mode to cause all meshed nodes to flash (battery/continuity check)
	else if(data[0] == DSP_CHECK)
	{
		i = 20000;
		LED_Off;
		while(i--);
		LED_BLUE_On;
		i = 20000;
		while(i--);
		LED_Off;
		i = 20000;
		while(i--);
		LED_Off;
	}
}

void Reliable_SendTest(void)
{
	unsigned char data[3];
	data[0] = DSP_RELIABLE;
	data[1] = 2;
	data[2] = 0;
	xlowpan_send(&XLOWPAN_ADDR_BCAST, data,3);
}
